#include "SensorDataFileInput.h"
#include <iostream>
#include <process.h>

SensorDataFileInput::SensorDataFileInput (const std::string &name,
		char* dataFile, const std::string &intrinsicFile,
		const std::string &extrinsicFile) :
		SensorDataInput (name, intrinsicFile, extrinsicFile),
		_hasNewFrame (false), _inStream (dataFile, std::ios::binary)
{
	if (!_inStream) {
		printf ("Cannot open the file %s", dataFile);
		throw (-1);
	}

	__sensorParameter.readFrom (_inStream);

	if (__sensorParameter.depthPixelWidth) {
		__depthMetaData = cvCreateImage (cvSize (
				__sensorParameter.frameWidth,
				__sensorParameter.frameHeight), IPL_DEPTH_16U, 1);
		cvZero (__depthMetaData);
	}
	if (__sensorParameter.rgbPixelWidth) {
		__rgbMetaData = cvCreateImage (cvSize (
				__sensorParameter.frameWidth,
				__sensorParameter.frameHeight), 8, 3);
		cvZero (__rgbMetaData);
	}

	_frameDataSize = __sensorParameter.frameSize * (
			__sensorParameter.depthPixelWidth + __sensorParameter.rgbPixelWidth);
	_buf = new char[_frameDataSize];
}

SensorDataFileInput::~SensorDataFileInput () {
	delete []_buf;
}


bool SensorDataFileInput::_grabFrame ()
{
	std::ostringstream sOut;
	clock_t start = clock ();
	_inStream.read (_buf, _frameDataSize);
	if (_inStream)
		_hasNewFrame = true;
	bool result = false;
	if (_hasNewFrame) {
		if (__depthMetaData) {
			memcpy (__depthMetaData->imageData, _buf,
					__sensorParameter.frameSize *
					__sensorParameter.depthPixelWidth);
			for (unsigned int v = 0; v < __sensorParameter.frameHeight; ++v)
				for (unsigned int u = 0; u < __sensorParameter.frameWidth; ++u)
					if (CV_IMAGE_ELEM (__depthMetaData, unsigned short, v, u) == 0)
						CV_IMAGE_ELEM (__depthMetaData, unsigned short, v, u) =
								__sensorParameter.frameDepth;
		}
		if (__rgbMetaData) {
			memcpy (__rgbMetaData->imageData,
					_buf + __sensorParameter.frameSize *
					__sensorParameter.depthPixelWidth,
					__sensorParameter.frameSize *
					__sensorParameter.rgbPixelWidth);
		}
		result = true;
	}
	clock_t end = clock ();
	sOut << getName () << ": New frame read: " << end - start << std::endl;
	OutputDebugStringA (sOut.str ().c_str ());
	return result;
}